#include "bsp_timer.h"





extern TIM_HandleTypeDef htim1,htim5,htim10,htim8,htim2;

//servo 0 - 1000
void Servo_Output(uint16_t *servo)
{
	uint8_t i;
	uint16_t tmp[4];
	for(i=0;i<4;i++)
	{
		tmp[i] = servo[i];
		tmp[i] = tmp[i]>2500?2500:tmp[i];
	}
	__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,tmp[0]);
	__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2,tmp[1]);
	__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_3,tmp[2]);
	__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_4,tmp[3]);
}

void Servo_Init(void)
{
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1);
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_2);
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_3);
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_4);
}


void Motor_Output(uint16_t pwm_kilo)
{
	float pwm = 0.f;
	pwm_kilo = pwm_kilo>1000?1000:pwm_kilo;
	pwm = 1.f * pwm_kilo / 1.912f;
	__HAL_TIM_SET_COMPARE(&htim5,TIM_CHANNEL_4,pwm+477);
}


void Motor_Init(void)
{
	HAL_TIM_PWM_Start(&htim5,TIM_CHANNEL_4);
}


void Heating_Init(void)
{
	HAL_TIM_PWM_Start(&htim10,TIM_CHANNEL_1);
}

void Heating_Output(uint16_t value)
{
	__HAL_TIM_SET_COMPARE(&htim10,TIM_CHANNEL_1,value>99?99:value);
}



void MotorSpeedMeasure_Init(void)
{
	HAL_TIM_Base_Start(&htim8);
}

int32_t MotorSpeed_GetMileage(void)
{
	int32_t cnt = 0;
	cnt = TIM8->CNT;
	return cnt;
}



uint32_t CalTaskTime_GetTick(void)
{
	return TIM1->CNT;
}

void CalTaskNum_Init(void)
{
	HAL_TIM_Base_Init(&htim2);
	HAL_TIM_Base_Start_IT(&htim2);
}

extern void task_monitor_set_timeflag(void);

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
	if(htim->Instance==TIM2)
	{
		__HAL_TIM_CLEAR_IT(&htim2,TIM_IT_UPDATE);
		task_monitor_set_timeflag();
	}
		
}




void RC_GetChannel3_Init(void)
{
	HAL_TIM_Base_Start(&htim8);
	HAL_TIM_IC_Start_IT(&htim8,TIM_CHANNEL_3);	
}

uint16_t IC_CCR[3] = {0,0,0};
uint16_t IC_Buffer[2] = {0,0xffff};
uint8_t start_flag = 0;


void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
{
	if(htim->Instance==TIM8)
	{
		__HAL_TIM_CLEAR_IT(&htim8,TIM_IT_CC3);
		if(!start_flag)
		{
			IC_CCR[0] = TIM8->CCR3;
			__HAL_TIM_SET_CAPTUREPOLARITY(&htim8,TIM_CHANNEL_3,TIM_ICPOLARITY_FALLING);
			start_flag = 2;
		}else if(start_flag == 1)
		{
			IC_CCR[1] = TIM8->CCR3;
			if(IC_CCR[1] > IC_CCR[0])
			{
				IC_Buffer[0] = IC_CCR[1] - IC_CCR[0] + 1;
			}else
			{
				IC_Buffer[0] = TIM8->ARR - IC_CCR[0] + IC_CCR[1] + 2;
			}
			__HAL_TIM_SET_CAPTUREPOLARITY(&htim8, TIM_CHANNEL_3, TIM_INPUTCHANNELPOLARITY_RISING);
			start_flag = 2;
		}else if(start_flag==2)
		{
			IC_CCR[2] = TIM8->CCR3;
			if(IC_CCR[2] > IC_CCR[0])
			{
				IC_Buffer[1] = IC_CCR[2] - IC_CCR[0] + 1;
			}else
			{
				IC_Buffer[1] = TIM8->ARR - IC_CCR[0] + IC_CCR[2] + 2;
			}
			IC_CCR[0] = IC_CCR[2];
			__HAL_TIM_SET_CAPTUREPOLARITY(&htim8, TIM_CHANNEL_3, TIM_INPUTCHANNELPOLARITY_FALLING);
			start_flag = 1;
		}
	}
}

float limit_Channel3_duty[2] = {4.77f, 10.44f};

uint16_t RC_GetChannel3Value(void)
{
	static float temp_freq = 0.f;
	float duty = 0.f;
	uint16_t channel3value = 0.f;
	temp_freq = 168.f*1000000/(TIM8->PSC+1) / (1.f*IC_Buffer[1]);
	temp_freq = Deathzoom(temp_freq,50.f,1.f);
	if(temp_freq != 50.f)
		return 0.f;
	
	duty = (1.f*IC_Buffer[0]) / (1.f*IC_Buffer[1])*100.f;
	channel3value = (duty - limit_Channel3_duty[0])/ \
									(limit_Channel3_duty[1] - limit_Channel3_duty[0]) * 1000;
	
	return channel3value;
}
